/*
 * This code is for a 4 channel transmitter of remote control RC
 * Using 1 nRF24L01 transceiver to another, on 2 devices acting as "nodes".
 * Referance: https://nrf24.github.io/RF24/
 * 
 * Connection:
 * NRF24L01+ PA: CE: Chip Enable; CSN: Chip Select Negtive
 * 
 * NRF24L01+ PA Connection with PRO Mini
 *   1 GND  - GND VCC  - 3.3V 
 *   3 CE   - D7  CSN  - D8
 *   5 SCK  - 13  MOSI - 11
 *   7 MISO - 12 IRQ  - None
 *   
 * NRF24L01+ PA Connection with MEGA
 *   1 GND  - GND VCC  - 3.3V 
 *   3 CE   - 7   CSN  - 8
 *   5 SCK  - 52  MOSI - 51
 *   7 MISO - 50  IRQ  - None
 *   
 * NRF24L01+ PA Connection with NANO
 *   1 GND  - GND VCC  - 3.3V
 *   3 CE   - D7  CSN  - D8
 *   5 SCK  - D13 MOSI - D11
 *   7 MISO - D12 IRQ  - None
 *   
 * 4 channel JoyStick connection with NANO as a transmitter
 *    Throttle        - A0
 *    Rudder(Roll)    - A1
 *    Pitch(Elevator) - A2
 *    Yaw (AIL)       - A3
 *    Throttle,Rudder, Elevator, Aileron  
 *  
 *    
 * History:
 * V0.0 KendinYap https://www.rcpano.net/2020/02/17/simple-and-cheap-radio-control-making-for-rc-models-diy-rc/
 * V0.1 25.06.2023 SH first adaption
 */
 
#include <SPI.h>
#include "printf.h"
#include "RF24.h"
#include "NonBlockingRtttl.h"
//#include "EEPROM.h"
#include "config.h"
//#include "crsf.h"
#include "led.h"
#include "tone.h"

#define DEBUG // if not commented out, Serial.print() is active! For debugging only!!

#define CE_PIN 7
#define CSN_PIN 8
// instantiate an object for the nRF24L01 transceiver
RF24 radio(CE_PIN, CSN_PIN);

// Let these addresses be used for the pair
uint8_t address[][6] = { "1Node", "2Node" };
// It is very helpful to think of an address as a path instead of as
// an identifying device destination

// to use different addresses on a pair of radios, we need a variable to
// uniquely identify which address this radio will use to transmit
bool radioNumber = 1;  // 0 uses address[0] to transmit, 1 uses address[1] to transmit

// Used to control whether this node is sending or receiving
bool role = false;  // true = TX role, false = RX role

// For this example, we'll be using a payload containing
// a single float number that will be incremented
// on every successful transmission
float payload = 0.0;

float batteryVoltage;

int currentSetting = 0;
int stickMoved = 0;
int stickInt = 0;
uint32_t stickMovedMillis = 0;

int previous_throttle = 191;


// Deifne 6-channel signals
struct Signal {
  byte throttle;
  byte pitch;
  byte roll;
  byte yaw;
  byte gyr; 
  byte pit;
};

Signal data;

void ResetData()
{
  data.throttle = 0; // Motor Stop (254/2=127)|(Signal lost position | 信号丢失时，关闭油门Throttle)
  data.pitch = 127; // Center |(Signal lost position | 俯仰通道Elevator)
  data.roll = 127; // Center |(Signal lost position | 横滚通道Aileron(中心点127))
  data.yaw = 127; // Center | (Signal lost position | 航向通道Rudder )
  data.gyr = 0; //第五通道 
  data.pit = 0; //第六通道
} // ResetData

bool checkStickStandstill(){
    // check if stick moved, warning after 10 minutes
    if(abs(previous_throttle - data.throttle) < 30){
        stickMoved = 0;
        //Serial.println(abs(previous_throttle - rcChannels[THROTTLE]));
    }else{
        previous_throttle = data.throttle;
        stickMovedMillis = millis();
        stickMoved = 1;
    }

    if (millis() - stickMovedMillis > STICK_ALARM_TIME){
       // Serial.println((millis() - stickMovedMillis));
        return true;
    }else{
        return false;
    }
} //end of checkStickMove

void setup() {

  Serial.begin(115200);
  while (!Serial) {
    // some boards need to wait to ensure access to serial over USB
  }

  // initialize the transceiver on the SPI bus
  if (!radio.begin()) {
    Serial.println(F("radio hardware is not responding!!"));
    while (1) {}  // hold in infinite loop
  }

  // print example's introductory prompt
  Serial.println(F("RF24/Transmitter starts"));

  // To set the radioNumber via the Serial monitor on startup
  //Serial.println(F("Which radio is this? Enter '0' or '1'. Defaults to '0'"));
  //while (!Serial.available()) {
    // wait for user input
  //}
  //char input = Serial.parseInt();
  char input = 0;
  radioNumber = input == 1;
  Serial.print(F("radioNumber = "));
  Serial.println((int)radioNumber);

  // Set the PA Level low to try preventing power supply related problems
  // because these examples are likely run with nodes in close proximity to
  // each other.
  // radio.setPALevel(RF24_PA_LOW);  // RF24_PA_MAX is default.

  // save on transmission time by setting the radio to only transmit the
  // number of bytes we need to transmit a float
  radio.setPayloadSize(sizeof(payload));  // float datatype occupies 4 bytes

  // set the TX address of the RX node into the TX pipe
  radio.openWritingPipe(address[radioNumber]);  // always uses pipe 0

  // set the RX address of the TX node into a RX pipe
  radio.openReadingPipe(1, address[!radioNumber]);  // using pipe 1

  // additional setup specific to the node's role
  if (role) {
    radio.stopListening();  // put radio in TX mode
  } else {
    radio.startListening();  // put radio in RX mode
  }

  // For debugging info
  // printf_begin();             // needed only once for printing details
  // radio.printDetails();       // (smaller) function that prints raw register values
  // radio.printPrettyDetails(); // (larger) function that prints human readable data

  analogReference(EXTERNAL);
  
  // initialize all Joy stick
  ResetData();

pinMode(DIGITAL_PIN_LED, OUTPUT);

#ifdef PASSIVE_BUZZER
    digitalWrite(DIGITAL_PIN_BUZZER, LOW); // BUZZER OFF
    if(STARTUP_MELODY!=""){
      rtttl::begin(DIGITAL_PIN_BUZZER, STARTUP_MELODY);
    }
#endif

#ifdef PASSIVE_BUZZER
    if(STARTUP_MELODY!="") {
      while( !rtttl::done() ) {
        rtttl::play();
      }
    }
    else {
     delay(2000); // Give enough time for uploading firmware (2 seconds)     
    }
#else  
    digitalWrite(DIGITAL_PIN_BUZZER, LOW); //BUZZER ON
    delay(100); // beep
    digitalWrite(DIGITAL_PIN_BUZZER, HIGH); //BUZZER OFF
    delay(200);
    digitalWrite(DIGITAL_PIN_BUZZER, LOW); //BUZZER ON
    delay(100); // beep
    digitalWrite(DIGITAL_PIN_BUZZER, HIGH); //BUZZER OFF
    delay(200);
    digitalWrite(DIGITAL_PIN_BUZZER, LOW); //BUZZER ON
    delay(200); // beep
    digitalWrite(DIGITAL_PIN_BUZZER, HIGH); //BUZZER OFF
    delay(2000); // Give enough time for uploading firmware (2 seconds)
#endif

    // inialize voltage:
    batteryVoltage = 0.0;
    
}  //end of setup

// Define for Joystick center and its borders
int mapJoystickValues(int val, int lower, int middle, int upper, bool reverse) {
  val = constrain(val, lower, upper);
  if ( val < middle )
    val = map(val, lower, middle, 0, 128);
  else
    val = map(val, middle, upper, 128, 255);
  return (reverse ? 255 - val : val);
} // end of mapJoystick

void loop() {
  // melody player needs to be first in the loop in order to play correctly.
  rtttl::play();

    // uint32_t currentMicros = micros(); //for CRSF
    if (currentSetting == 0) {
      blinkLED(DIGITAL_PIN_LED, -1);
    } //Turn on LED when not linked
    

    // Read Voltage
    batteryVoltage = analogRead(VOLTAGE_READ_PIN) / VOLTAGE_SCALE; //98.5
    #ifdef DEBUG
      Serial.print("batteryVoltage: ");
      Serial.print(batteryVoltage);
      Serial.print("V ");
    #endif

    if (batteryVoltage < WARNING_VOLTAGE && batteryVoltage >= BEEPING_VOLTAGE) {
        blinkLED(DIGITAL_PIN_LED, 1000);
    }else if(batteryVoltage < BEEPING_VOLTAGE && batteryVoltage >= ON_USB){
        blinkLED(DIGITAL_PIN_LED, 250);
        playingTones(2);
    }else if(currentSetting == 3){
        blinkLED(DIGITAL_PIN_LED, 100);  // Bind (fast flash)
    }else if(currentSetting == 4){
        blinkLED(DIGITAL_PIN_LED, 500); // Wifi (slow flash)
    }

    if (checkStickStandstill() == true){
      #ifdef DEBUG
        Serial.print("Stick is standstill for a while");
      #endif
        blinkLED(DIGITAL_PIN_LED, 100);
        playingTones(5);
    }
#ifdef PASSIVE_BUZZER
    else { // Stop the stick warning tone if you move the sticks
      if ( rtttl::isPlaying() ) {
        rtttl::stop();
      }
    }
#endif
  // Print value from analog input
  //Serial.print("\t");Serial.print(analogRead(A0));
  //Serial.print("\t");Serial.print(analogRead(A1));
  //Serial.print("\t");Serial.print(analogRead(A2));
  //Serial.print("\t");Serial.print(analogRead(A3));
  //Serial.print("\t");
  // Control Stick Calibration to 0-255
  // Setting may be required for the correct values of the control levers.
  data.throttle = mapJoystickValues(analogRead(analogInPinThrottle), 130, 530, 890,false);
  data.yaw = mapJoystickValues(analogRead(analogInPinRudder), 124, 529, 894, true);      // "true" or "false" for servo direction
  data.pitch = mapJoystickValues(analogRead(analogInPinElevator), 103, 517, 903, true);     // "true" or "false" for servo direction
  data.roll = mapJoystickValues(analogRead(analogInPinAileron), 93, 496, 888, true);       // "true" or "false" for servo direction

  #ifdef DEBUG
    Serial.print("A-E-T-R"); //Ail>>roll, ELE>>pitch, THR, RUD>>yaw 
    Serial.print("\t");Serial.print(data.roll);
    Serial.print("\t");Serial.print(data.pitch);
    Serial.print("\t");Serial.print(data.throttle);
    Serial.print("\t");Serial.println(data.yaw);    
    //Serial.print("\t");
  #endif

  if (role) {
    // This device is a TX node
    unsigned long start_timer = micros();                // start the timer
    bool report = radio.write(&data, sizeof(Signal)); // transmit & save the report
    unsigned long end_timer = micros();                  // end the timer

    if (report) {
      // Serial.print(F("Transmission successful! "));  // payload was delivered
      blinkLED(DIGITAL_PIN_LED, 500);
      Serial.print(F("Time to transmit = "));
      Serial.print(end_timer - start_timer);  // print the timer result
      Serial.print(F(" us. Sent: "));
      //Serial.println(payload);  // print payload sent
      //payload += 0.01;          // increment float payload
      currentSetting = 4; //linked
    } else {
      Serial.println(F("Transmission failed or timed out"));  // payload was not delivered
      currentSetting = 0; //not linked
    }

    // to make this example readable in the serial monitor
    delay(100);  // slow transmissions down by 1 second
 
  }  // role

  //if (Serial.available()) {
    // change the role via the serial monitor

    // char c = toupper(Serial.read());
    char c = 'T';
    if (c == 'T' && !role) {
      // Become the TX node

      role = true;
      // Serial.println(F("*** CHANGING TO TRANSMIT ROLE -- PRESS 'R' TO SWITCH BACK"));
      radio.stopListening();
    }
}  // end of Loop
